#ifndef DATA_QUEUE_HPP
#define DATA_QUEUE_HPP

#include <queue>
#include <mutex>
#include <chrono>
#include <iterator>

using namespace std::literals;

namespace hnurm
{
    template <typename DataType>
    class TimeQueue
    {

        using Us = std::chrono::microseconds;
        using tp = std::chrono::time_point<std::chrono::steady_clock>;

    private:
        int size_; /**< 队列的最大大小 */
        std::queue<std::pair<DataType, tp>> data_queue; /**< 数据队列 */
        std::timed_mutex mtx; /**< 带有超时功能的互斥锁 */

    public:
        /**
         * @brief 时间队列的构造函数
         *
         * @param max_size 队列的最大大小，默认为1000
         */
        explicit TimeQueue(int max_size = 1000) : size_(max_size) {}

        /**
         * @brief 从时间队列中取出一个距离传入时间最近的数据，并将其从队列中删除
         *
         * @param data 要获取的数据
         * @param time 传入的时间
         * @return 如果成功取出数据并删除，则返回true，否则返回false
         */
        bool Get(DataType &data, tp &time);

        /**
         * @brief 向时间队列中更新数据
         *
         * @param data 要更新的数据
         * @param time 数据的更新时间
         * @return 如果成功更新数据，则返回true，否则返回false
         */
        bool Update(const DataType &data, tp &time);
    };

    /**
     * @brief 从时间队列中取出一个距离传入时间最近的数据,并将其从队列中删除
     *
     * @param data 要获取的数据
     * @param time 传入的时间
     * @return
     */
    template <typename DataType>
    bool TimeQueue<DataType>::Get(DataType &data, tp &time)
    {
        if (mtx.try_lock_for(2us)) {
            std::lock_guard<std::timed_mutex> guard(mtx, std::adopt_lock);
            Us delta_t;
            if (!data_queue.empty()) {          // 队列不为空
                data = data_queue.front().first;// 初始化时间差
                delta_t = std::chrono::duration_cast<Us>(std::chrono::abs(data_queue.front().second - time));
                data_queue.pop();
                while (!data_queue.empty())// 队列不为空，继续寻找更近的数据
                {
                    if (abs(data_queue.front().second - time) < delta_t)// 找到更近的数据
                    {
                        data = data_queue.front().first;
                        delta_t = std::chrono::duration_cast<Us>(std::chrono::abs(data_queue.front().second - time));
                        data_queue.pop();
                        continue;// 下一轮可能还有更近的数据
                    }
                    return true;// 判断不通过，说明已经不能再找到更近的数据了
                }
                return true;// 队列已经为空也直接返回
            }
        }
        return false; // 锁不上||队列为空
    }

    /**
     * @brief 向时间队列中更新数据
     *
     * @param data 要更新的数据
     * @param time 数据的更新时间
     * @return
     */
    template <typename DataType>
    bool TimeQueue<DataType>::Update(const DataType &data, tp &time)
    {
        if (mtx.try_lock_for(2ms))
        {
            std::lock_guard<std::timed_mutex> guard(mtx, std::adopt_lock);
            while (data_queue.size() >= size_) // 队列满了,清除最早的数据直到队列有空位
                data_queue.pop();
            data_queue.push(std::make_pair(data, time));
            return true;
        }
        return false;
    }

} // namespace

#endif // !DATA_QUEUE_HPP